#pragma once

#include <Poco/Types.h>
#include <string>
#include <math.h>

#pragma region Struct
struct Point3D
{
    Point3D() : x(0), y(0), z(0) {}
    Point3D(double xx, double yy, double zz) : x(xx), y(yy), z(zz) {}

    double x;
    double y;
    double z;

    Point3D operator-(const Point3D &other) const
    {
        return Point3D(x - other.x, y - other.y, z - other.z);
    }

    Point3D operator+(const Point3D &other) const
    {
        return Point3D(x + other.x, y + other.y, z + other.z);
    }

    Point3D operator*(const double ratio) const
    {
        return Point3D(x * ratio, y * ratio, z * ratio);
    }

    Point3D operator/(const double ratio) const
    {
        return Point3D(x / ratio, y / ratio, z / ratio);
    }

    double operator*(const Point3D &other) const
    {
        return x * other.x + y * other.y + z * other.z;
    }

    Point3D operator^(const Point3D &other) const
    {
        return Point3D(y * other.z - other.y * z, -(x * other.z - other.x * z), x * other.y - other.x * y);
    }

    double length()
    {
        return sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2));
    }

    void normalize()
    {
        double len = length();
        x = x / len;
        y = y / len;
        z = z / len;
    }
};

struct LidarInfo
{
    LidarInfo() : port(0), location(Point3D()), heading(0) {}

    int port;
    Point3D location;
    double heading;
};
#pragma endregion